#include <GL/glut.h>
#include "Matrix3.h"
#include "Matrix4.h"
#include "Vec2f.h"
#include "Vec3f.h"
#include "Quaternion.h"
#include "Camera.h"

const static float STEP_SCALE = 10.1f;
const static int MARGIN = 10;

Camera::Camera() : mPosition(0, 1000, 1000)
{
    mOrientation = Quaternion::IDENTITY; 
    rotate(Vector3f::UNIT_X, Radian(Degree(-45)));
}


Camera::Camera(const Vector3f& Pos, const Quaternion& quat) 
{
    mPosition = Pos;
    mOrientation = quat;
}

const Vector3f& Camera::getPosition(void) const {
    return mPosition;
}

void Camera::move(const Vector3f& vec) {
    mPosition += vec;
}

void Camera::roll(const Radian& angle) {
    Vector3f zAxis = mOrientation * Vector3f::UNIT_Z;
    rotate(zAxis, angle);
}

void Camera::yaw(const Radian& angle) {
    Vector3f yAxis = mOrientation * Vector3f::UNIT_Y;
    rotate(yAxis, angle);

}
void Camera::pitch(const Radian& angle) {
    Vector3f xAxis = mOrientation * Vector3f::UNIT_X;
    rotate(xAxis, angle);
}

void Camera::rotate(const Vector3f& axis, const Radian& angle) {
    Quaternion q;
    q.FromAngleAxis(angle, axis);
    rotate(q);
}

void Camera::rotate(const Quaternion& q) {
    Quaternion qnorm = q;
    qnorm.normalise();
    mOrientation = qnorm * mOrientation;
}

bool Camera::OnKeyboard(int Key)
{
    std::cerr << "Camera::OnKeyboard." << std::endl;
    bool Ret = false;

    switch (Key) {
        case 'f':
        case 'F':
            {
                Vector3f vec(0, 0, -10);
                Vector3f vecForward = mOrientation * vec;
                move(vecForward);
            }
            break;
        case 'b':
        case 'B':
            {
                Vector3f vec(0, 0, 10);
                Vector3f vecForward = mOrientation * vec;
                move(vecForward);
            }
            break;
        case 'u': case 'U':
            {
                Vector3f vec(0, 10, 0);
                Vector3f vecNew= mOrientation * vec;
                move(vecNew);
            }
            break;
        case 'd': case 'D':
            {
                Vector3f vec(0, -10, 0);
                Vector3f vecNew= mOrientation * vec;
                move(vecNew);
            }
            break;
        case 'l': case 'L':
            {
                Vector3f vec(10, 0, 0);
                Vector3f vecNew= mOrientation * vec;
                move(vecNew);
            }
            break;
        case 'r': case 'R':
            {
                Vector3f vec(-10, 0, 0);
                Vector3f vecNew= mOrientation * vec;
                move(vecNew);
            }
            break;
        default:
            break;
    }
}


bool Camera::OnSpecialKeyboard(int Key) {
    bool Ret = false;

    switch(Key) {
        case GLUT_KEY_UP:
        {
            pitch(Radian(Degree(5)));
            Ret = true;
        }
        break;

    case GLUT_KEY_DOWN:
        {
            pitch(Radian(Degree(-5)));
            Ret = true;
        }
        break;

    case GLUT_KEY_LEFT:
        {
            yaw(Radian(Degree(-5)));
            Ret = true;
        }
        break;

    case GLUT_KEY_RIGHT:
        {
            yaw(Radian(Degree(5)));
            Ret = true;
        }
        break;
    }

    return Ret;
}


void Camera::OnMouse(int x, int y)
{
}


void Camera::OnRender()
{
    Matrix3 m3 = Matrix3::IDENTITY;;

    mOrientation.ToRotationMatrix(m3);

    Matrix4 m4(m3);
    glMultMatrixf(&m4[0][0]);
    glTranslatef(-mPosition.x, -mPosition.y, -mPosition.z);
}

